#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv/highgui.h>
#include "opencv2/gpu/gpu.hpp"
#include <koala_hog/bbox.h>

namespace enc = sensor_msgs::image_encodings;
long int counter =  1;

static const char WINDOW[] = "Image window";

class ImageConverter
{
  ros::NodeHandle nh_;
  image_transport::ImageTransport it_;
  image_transport::Subscriber image_sub_;
  image_transport::Publisher image_pub_;
  ros::Publisher bounding_box_pub_;

public:
  ImageConverter()
    : it_(nh_)
  {
    image_pub_ = it_.advertise("out", 1);
    image_sub_ = it_.subscribe("/camera/rgb/image_rect_color", 1, &ImageConverter::imageCb, this);
    bounding_box_pub_ = nh_.advertise<koala_hog::bbox>("hog_bounding_box", 1);

    cv::namedWindow(WINDOW, CV_WINDOW_NORMAL);
    cv::namedWindow("CROPPED", CV_WINDOW_NORMAL);

  }

  ~ImageConverter()
  {
    cv::destroyWindow(WINDOW);
    cv::destroyWindow("CROPPED");
  }

  void imageCb(const sensor_msgs::ImageConstPtr& msg)
  {
    cv_bridge::CvImagePtr cv_ptr;
    try
    {
      cv_ptr = cv_bridge::toCvCopy(msg, enc::RGB8);
    }
    catch (cv_bridge::Exception& e)
    {
      ROS_ERROR("cv_bridge exception: %s", e.what());
      return;
    }

    // Initialize GPU Mats
    cv::gpu::GpuMat src_gpu, mono_gpu, convertedImg;

    // Initialize HOG classifier and set descriptor class
    cv::gpu::HOGDescriptor hog;
    hog.setSVMDetector(cv::gpu::HOGDescriptor::getDefaultPeopleDetector());

    // Create vector of bounding boxes
    std::vector<cv::Rect> found;

    // ROTATE IMAGE BY 90 DEG
    cv::Mat rotated;
    cv::transpose(cv_ptr->image, rotated);
    cv::flip(rotated, rotated, -1);
    cv::flip(rotated, rotated, 1);

    // Convert FUERTE kinect image to ELECTRIC OpenCV GPU image
    //src_gpu.upload(cv_ptr->image);
    src_gpu.upload(rotated);
    cv::gpu::cvtColor(src_gpu, convertedImg, CV_BGR2RGB);
    cv::gpu::cvtColor(convertedImg, mono_gpu, CV_RGB2GRAY);
    convertedImg.download(cv_ptr->image);

    // Detect pedestrains
    hog.detectMultiScale(mono_gpu, found);

    // Draw detected pedestrians in image
    for(unsigned i = 0; i < found.size(); i++) {
        cv::Rect r = found[i];
        if(r.height > r.width && r.area()>1000)
        {
            cv::Mat croppedImage = cv_ptr->image(r);
            int rgb[6];// = colour(croppedImage);
            colour(croppedImage, rgb);
            koala_hog::bbox output;

            output.rl = rgb[0];
            output.gl = rgb[1];
            output.bl = rgb[2];
            output.rt = rgb[3];
            output.gt = rgb[4];
            output.bt = rgb[5];
            output.header = msg->header;
            output.x = r.x;
            output.y = r.y;
            output.height = r.height;
            output.width = r.width;
            cv::rectangle(cv_ptr->image, r.tl(), r.br(), cv::Scalar(0,255,0), 2);
            bounding_box_pub_.publish(output);
        }
    }

    cv::imshow(WINDOW,cv_ptr->image);
    cv::waitKey(3);
  }

  int* colour(cv::Mat image, int* colours)
  {
    int center_torso_x = image.cols/2;
    int center_torso_y = image.rows/2.9;

    int center_legs_x = image.cols/2;
    int center_legs_y = image.rows/1.95;

    int thickness = -1;
    int lineType = 8;

    int R_torso =0;
    int G_torso =0;
    int B_torso =0;

    int R_leg =0;
    int G_leg =0;
    int B_leg =0;

    for(int i = center_torso_x-5; i<center_torso_x+5;i++)
    {
        for (int j = center_legs_y-5; j < center_legs_y+5; ++j) {
            cv::Vec3b bgrPixel = image.at<cv::Vec3b>(j, i);
            R_leg += bgrPixel[2];
            G_leg += bgrPixel[1];
            B_leg += bgrPixel[0];
        }

        for (int s = center_torso_y-5; s < center_torso_y+5; ++s) {
            cv::Vec3b bgrPixel = image.at<cv::Vec3b>(s, i);
            R_torso += bgrPixel[2];
            G_torso += bgrPixel[1];
            B_torso += bgrPixel[0];
        }
    }

    R_leg = (R_leg/100);
    G_leg = (G_leg/100);
    B_leg = (B_leg/100);

    R_torso = (R_torso/100);
    G_torso = (G_torso/100);
    B_torso = (B_torso/100);

//    std::cout << "R: " << (R_leg/100) << " G: " << (G_leg/100) << " B: " << (B_leg/100) << std::endl;
//    std::cout << "R: " << (R_torso/100) << " G: " << (G_torso/100) << " B: " << (B_torso/100) << std::endl;
//    std::cout << counter <<std::endl;

        cv::circle( image,
                cv::Point(center_torso_x,center_torso_y),
                10,
                cv::Scalar( 0, 0, 255 ),
                thickness,
                lineType );

        cv::circle( image,
                cv::Point(center_legs_x,center_legs_y),
                10,
                cv::Scalar( 0, 0, 255 ),
                thickness,
                lineType );
    cv::imshow("CROPPED", image);

    //int colours[6];
    colours[0] = R_leg;
    colours[1] = G_leg;
    colours[2] = B_leg;
    colours[3] = R_torso;
    colours[4] = G_torso;
    colours[5] = B_torso;

    return colours;

  }
};

int main(int argc, char** argv)
{
  ros::init(argc, argv, "image_converter");
  ImageConverter ic;
  ros::spin();
  return 0;
}
